#ifndef ROBORTS_DECISION_RECOVERY_H
#define ROBORTS_DECISION_RECOVERY_H

#include "../blackboard/blackboard.h"
#include "costmap/costmap_interface.h"
#include "../executor/chassis_executor.h"
#include "math.h"
#define pi 3.14159265
namespace roborts_decision{
class Recovery {
 public:
  Recovery(ChassisExecutor* &chassis_executor,Blackboard* &blackboard):blackboard_(blackboard),chassis_executor_(chassis_executor){
      //ros::NodeHandle n;
      //result_pub = n.advertise<geometry_msgs::PoseStamped>("bfs",1000);
       escape_spd = 1.0;
    }
  
  ~Recovery() = default;

  bool Run(){
      auto scan = blackboard_->GetScan();
      std::vector<double> angle;
      double min_value = 0.3;
      int i = 0;
      for(i=0;i<scan.ranges.size();i++)
      {
          if((scan.ranges[i]<min_value&&scan.ranges[i]>0.15)&&(i<500||i>900))  //clear the points from gimbal
          {
            angle.emplace_back(scan.angle_min + i*scan.angle_increment);
          }
      }
      if(angle.size()>0)
      {
        double avg_angle = angle_avg(angle);
        roborts_msgs::TwistAccel whirl_vel_;
        whirl_vel_.twist.linear.x = escape_spd*cos(avg_angle);
        whirl_vel_.twist.linear.y = escape_spd*sin(avg_angle);
        chassis_executor_->Execute(whirl_vel_);
      }
      else
      {
        roborts_msgs::TwistAccel whirl_vel_;
        whirl_vel_.twist.linear.x = 0;
        whirl_vel_.twist.linear.y = 0;
        chassis_executor_->Execute(whirl_vel_);
      }

      return true;
  }

  bool PointAvaible(unsigned int mx,unsigned int my)
  {
     if(mx<costmap_2d_->GetSizeXCell()&&mx>0&&my<costmap_2d_->GetSizeYCell()&&my>0)
        return true;
     else
        return false;
  }

 private:
  Blackboard* const blackboard_;
  ChassisExecutor* const chassis_executor_;
  double escape_spd;
  const roborts_costmap::Costmap2D* costmap_2d_;
  const unsigned char* char_map_;
  std::queue<std::pair<int,int>> iter_queue;
  int max_iter;

  double angle_diff(double x2,double x1)
  {
    double diff = 0;
    diff = x2-x1+180; 
    if (diff < 0) {  
        diff = diff + 360 - 180;  
    } else if (diff > 360) {  
        diff = diff - 360 - 180;  
    } else {  
        diff = diff - 180;  
    }
    return diff;  
  }

  double angle_avg(std::vector<double>& angle)
  {
      double sum = 0;
      for (int i=1; i<angle.size(); i++) {
        sum+=angle[i];
      } 
      double avg = sum/angle.size();
      double add_pi = 0;
      double not_add  = 0;    //计算是否需要加pi
      for(int i=0;i<angle.size();i++)
      {
        
        double add_bias = std::min(abs(angle[i]-(avg+pi)),abs(angle[i]-(avg-pi)));
        double normal_bias = std::min({abs(angle[i]-avg),abs(angle[i]-avg-2*pi),abs(angle[i]-avg+2*pi)});
        add_pi += add_bias;
        not_add += normal_bias;
      }

      if(add_pi<not_add)
      {
        avg+=pi;
      }
      return avg;
  }
  //ros::Publisher result_pub;
};
}
#endif
